#include "MS42DDC.h"
#include "string.h"
#include "math.h"

//
#include "stm32f1xx_hal.h"
#include "dma.h"
#include "usart.h"

#define USE_UART huart2
#define TIMEOUT 1000
//

#define COM_HEAD            0x7B // 帧头
#define COM_END             0x7D // 帧尾
#define MS42_ServiceAddress 0x01 // 设备地址

#define DefaultCurrent      1500
#define MaxSpeed            30
#define DebugSpeed          30

static uint8_t RawDataString[16] = {0};

static MS42Data_u DefaultWriteData = {
    .write.head = COM_HEAD,
    .write.address = MS42_ServiceAddress,
    .write.seg = segment32,
    .write.end = COM_END,
};

static MS42Data_u DefaultReadData = {
    .read.address = MS42_ServiceAddress,
}; // 读数据可以不初始化

static void UARTTransmit(MS42Data_u *data)
{
    HAL_UART_Transmit_DMA(&USE_UART, (uint8_t *)data, MS42_CommandLen);
}

static void UARTReceive()
{
    HAL_UARTEx_ReceiveToIdle_DMA(&USE_UART, RawDataString, sizeof(RawDataString));
    __HAL_DMA_DISABLE_IT(USE_UART.hdmarx, DMA_IT_HT);
}

/**
 * @brief 阻塞式串口发送
 *
 * @param data
 * @param size
 * @return uint8_t 1成功0失败
 */
static uint8_t UARTTransmit_Blocking(MS42Data_u *data)
{
    if (HAL_UART_Transmit(&USE_UART, (uint8_t *)data, MS42_CommandLen, TIMEOUT) == HAL_OK)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

/**
 * @brief 阻塞式串口接收
 *
 * @return uint8_t 1成功0失败
 */
static uint8_t UARTReceive_Blocking()
{
    if (HAL_UART_Receive(&USE_UART, RawDataString, MS42_CommandLen, TIMEOUT) == HAL_OK)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

/**
 * @brief 计算BCC校验，为前面九个字节的异或和
 *
 * @param str
 * @return uint8_t
 */
static uint8_t CaculateBCC(MS42Data_u *uni)
{
    uint8_t bcc = 0;
    bcc = uni->u8[0];
    for (uint8_t i = 1; i < 9; i++)
    {
        bcc ^= uni->u8[i];
    }
    return bcc;
}

static uint8_t UnpackData(MS42Data_u *data, uint8_t str[])
{
    if (CaculateBCC((MS42Data_u *)str) == 0) // 返回共9位，全部异或为0
    {
        memcpy(data, str, MS42_CommandLen);
        return 1;
    }
    else
    {
        return 0;
    }
}

static uint16_t SwapU16(uint16_t data)
{
    return ((data << 8) | (data >> 8));
}

void MS42SpeedModeSetSpeed(MS42Data_u *data, float speed)
{
    data->write.controlType = SpeedControl;
    if (speed > 0)
    {
        data->write.dir = CW;
    }
    else
    {
        data->write.dir = CCW;
    }
    speed = fabs(speed);
    if (speed > MaxSpeed)
        speed = MaxSpeed;
    data->write.speed_radps = SwapU16((uint16_t)(speed * 10));
    data->write.pos.positionMult10 = 0;
    data->write.BCC = CaculateBCC(data);
}

void MS42SpeedModeSetSpeed_Default(float speed)
{
    MS42SpeedModeSetSpeed(&DefaultWriteData, speed);
}

void MS42ForceModeSetCurrent(MS42Data_u *data, uint16_t ma, uint16_t speed)
{
    data->write.controlType = ForceControl;
    if (speed > 0)
    {
        data->write.dir = CW;
    }
    else
    {
        data->write.dir = CCW;
    }
    speed = fabs(speed);
    data->write.speed_radps = SwapU16((uint16_t)(speed * 10));
    data->write.pos.mA = SwapU16(ma);
    data->write.BCC = CaculateBCC(data);
}

void MS42ForceModeSetCurrent_Default(uint16_t ma, uint16_t speed)
{
    MS42ForceModeSetCurrent(&DefaultWriteData, ma, speed);
}

void MS42AbsAngleModeSetAngle(MS42Data_u *data, float angle, float speed)
{
    data->write.controlType = AbsAngleControl;
    if (speed > 0)
    {
        data->write.dir = CW;
    }
    else
    {
        data->write.dir = CCW;
    }
    speed = fabs(speed);
    data->write.speed_radps = SwapU16((uint16_t)(speed * 10));
    data->write.pos.absAngleMult10 = SwapU16((uint16_t)(angle * 10));
    data->write.BCC = CaculateBCC(data);
}

void MS42AbsAngleModeSetAngle_Default(float angle, float speed)
{
    MS42AbsAngleModeSetAngle(&DefaultWriteData, angle, speed);
}

void MS42PositionModeSetPosition(MS42Data_u *data, float pos, float speed)
{
    data->write.controlType = PositionControl;
    if (speed > 0)
    {
        data->write.dir = CW;
    }
    else
    {
        data->write.dir = CCW;
    }
    speed = fabs(speed);
    data->write.speed_radps = SwapU16((uint16_t)(speed * 10));
    data->write.pos.positionMult10 = SwapU16((uint16_t)(pos * 10));
    data->write.BCC = CaculateBCC(data);
}

void MS42PositionModeSetPosition_Default(float pos, float speed)
{
    MS42PositionModeSetPosition(&DefaultWriteData, pos, speed);
}

/**
 * @brief UART DMA接收完一帧数据的回调
 *
 */
void MS42UARTReceiveCallback(void)
{
    // 电机数据回传50ms一次
    UnpackData(&DefaultReadData, RawDataString);
    UARTReceive();
}

/**
 * @brief 循环控制的回调，20Hz调用频率
 *
 */
void MS42CycleCallback(void)
{
    // UARTTransmit(&DefaultWriteData);
}

/**
 * @brief
 *
 */
void MS42UARTTransmitCallback_Default(void)
{
    UARTTransmit(&DefaultWriteData);
}

/**
 * @brief 初始化MS42电机
 *
 */
void MS42Init(void)
{
    // 设置电流并转动一次
    HAL_Delay(50);
    MS42ForceModeSetCurrent_Default(DefaultCurrent, DebugSpeed);
    UARTTransmit_Blocking(&DefaultWriteData);
    HAL_Delay(50);
    MS42PositionModeSetPosition_Default(3600, DebugSpeed);
    UARTTransmit_Blocking(&DefaultWriteData);
    // MS42SpeedModeSetSpeed_Default(-DebugSpeed);
    // UARTTransmit_Blocking(&DefaultWriteData);
    // UARTReceive();
}
